﻿#include "locomotion_header.h"
#include "gait_math.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#define SINGLE_LEG_TEST 0//WS Ϊ0
float lf_spd = 0.4;
float td_spd = 0.4;
#if !EN_PLAN_USE_JERK
float lf_spd_trot = 1.2;
float td_spd_trot = 0.8;
#else
#if EN_AUTO_FTORT
float lf_spd_trot = 0.8;
float td_spd_trot = 0.8;
#else
float lf_spd_trot = 0.6;
float td_spd_trot = 0.6;
#endif
#endif
float lf_spd_walk = 0.04;
float td_spd_walk = 0.04;

float lf_spd_back_x = 0.00;
#if USE_NAJIA_WALK
float lf_offz = 0.7;
#else
float lf_offz = 0.025;
#endif
float td_offz = 0.025;

char EN_Y_SLIP = 1;//
char EN_TD_XOFF = 1;
char EN_SLIP_CONTROL = 1;//
char EN_LIFT_Y_CONTROL = 0;
char EN_TD_Y_CONTROL = 0;
float k_spd_err = 55;//

float safe_bind_win[2] = { 0.1,0.035 };

float max_end_spd = 2;
float spd_end_test[3] = { 0,0,0 };
float sw_time_set = 8;
float EN_SPDN_TRIG[2] = { 1,1 };//{1,1}摆动跟随地面速度

float pd_y[2] = { 0.19,0.08 };//{0.09,0.03};

float k_y_roll = 0.00086;

float kp_rc = 0;
float kp_force = 0;
float yaw_trig_k = 0;//-0.025;//0.05;
float FLT_SW_END = 0;
float trig_z_flag = 99;//99;//0; 1 99   -1依据地形缩腿逆解  99柔顺
float td_time = 0;
float lf_time = 0;
float spd_kk[2] = { 1,1 };
float sw_z[4] = { 0 };
float ground_att[2] = { 0,0 };

char leg_lock_n_trigt[4] = { 0,0,0,0 };
char leg_lock_n_trig_regt[4] = { 0,0,0,0 };
END_POS end_lock_nt[4];

char cnt_reset_pos[4] = { 0 };
float cnt_online_plan[4] = { 0 };

float ground_pitch_off[2] = { 0.25,0.25 };
int vision_sw_enable[4] = { 0 };

float sw_t[4] = { 0.4,0.4,0.4,0.4 };
float st_t[4] = { 0.2,0.2,0.2,0.2 };
void webots_draw_planning_traj(VMC* in, float dt) {

	END_POS epos, epos_nn;
	int temp = 0;
	int id = in->param.id;
	int cnt_draw = 0;
	float time_trig = 0;
	float d_time = (sw_t[id] - st_t[id] - td_time - lf_time) / 30;
	while (time_trig < sw_t[id] - st_t[id] - td_time - lf_time) {
		time_trig += d_time;

		if (EN_PLAN_USE_JERK == 5)
			get_swing_jerk_5point(in, time_trig);
		else
			get_swing_jerk_3point(in, time_trig);

		webots_draw.sw_tar_traj[id][cnt_draw].x = in->param.end_planner.pos_now[Xr];
		webots_draw.sw_tar_traj[id][cnt_draw].y = in->param.end_planner.pos_now[Yr] + vmc_all.W / 2 * in->flag_rl;
		webots_draw.sw_tar_traj[id][cnt_draw].z = LIMIT(in->param.end_planner.pos_now[Zr],
			MAX_Z + in->flag_fb*vmc_all.H / 2 * sind(vmc_all.ground_att_est[PITr]),
			MIN_Z + in->flag_fb*vmc_all.H / 2 * sind(vmc_all.ground_att_est[PITr]));
		cnt_draw++;
	}
	webots_draw.sw_traj_cnt[id] = cnt_draw;
}


void cal_vir_leg_pos(VMC* in, END_POS *td_vir_n, END_POS *lf_vir_n)//
{
	char id_side_same = 0;//
	char id_other_side = 0;//
	/*2    0
	  3    1
	*/
	switch (in->param.id)
	{
	case 0:
		id_side_same = 1;
		id_other_side = 2;
		break;
	case 1:
		id_side_same = 0;
		id_other_side = 3;
		break;
	case 2:
		id_side_same = 3;
		id_other_side = 0;
		break;
	case 3:
		id_side_same = 2;
		id_other_side = 1;
		break;
	}
#if SINGLE_LEG_TEST
	END_POS ankle_pos_b, ankle_pos_n;
	ankle_pos_b.x = in->flag_fb*vmc_all.H / 2;
	ankle_pos_b.y = in->flag_rl*vmc_all.W / 2;
	ankle_pos_b.z = 0;

	converV_b_to_n(ankle_pos_b.x, ankle_pos_b.y, 0,
		&ankle_pos_n.x, &ankle_pos_n.y, &ankle_pos_n.z);

	td_vir_n->x = in->epos_td_hn.x;
	td_vir_n->y = in->epos_td_hn.y;
	td_vir_n->z = in->epos_td_hn.z;
	lf_vir_n->x = in->epos_lf_hn.x;
	lf_vir_n->y = in->epos_lf_hn.y;
	lf_vir_n->z = in->epos_lf_hn.z;
#else
	td_vir_n->x = vmc[id_side_same].epos_td_n.x / 2 + vmc[id_other_side].epos_td_n.x / 2;
	td_vir_n->y = vmc[id_side_same].epos_td_n.y / 2 + vmc[id_other_side].epos_td_n.y / 2;
	td_vir_n->z = vmc[id_side_same].epos_td_n.z / 2 + vmc[id_other_side].epos_td_n.z / 2;

	lf_vir_n->x = vmc[id_side_same].epos_lf_n.x / 2 + vmc[id_other_side].epos_lf_n.x / 2;
	lf_vir_n->y = vmc[id_side_same].epos_lf_n.y / 2 + vmc[id_other_side].epos_lf_n.y / 2;
	lf_vir_n->z = vmc[id_side_same].epos_lf_n.z / 2 + vmc[id_other_side].epos_lf_n.z / 2;
#endif
}

float tar_spd[3];
END_POS rotate_correct[4];
void cal_tar_end_pos_n_v1_sel(char leg, char recal, float dt)//
{
	float att_off = 0;
	float cog_off = 0;
	float spd_off[2] = { 0 }, acc_off[2] = { 0 };
	float spd_off_all[2] = { 0 }, cog_off_all[2] = { 0 };
	float size_off[2] = { 0 };

	float cof_off_all_use;
	float yaw_off[2];
	float temp_h = 0;
	int i = 0, j = 0;
	END_POS tar_epos_slip_ncom;
	END_POS end_epos_gn[4];

	tar_spd[Xr] = vmc_all.param.tar_spd_use_rc.x;
	tar_spd[Yr] = vmc_all.param.tar_spd_use_rc.y;

	Vect3 temp_g, temp_n;
	temp_n.x = tar_spd[Xr];
	temp_n.y = tar_spd[Yr];
	temp_n.z = 0;
	converV_n_to_g(temp_n, &temp_g);
	tar_spd[Xr] = temp_g.x;
	tar_spd[Yr] = temp_g.y;


	//----------------Raibert
	  //###############################################################################
	float w_ff = 0.35;
	int x_off_inv = 1;
	if (vmc_all.side_flip == 1)
		x_off_inv = -1;
	if (fabs(tar_spd[Xr]) > 0.01) {
		if ((tar_spd[Xr]) >= 0)
			spd_off_all[Xr] = spd_off_all[Xr] * w_ff + (1 - w_ff)*vmc_all.param.cof_off_all[F];
		else
			spd_off_all[Xr] = spd_off_all[Xr] * w_ff + (1 - w_ff)*vmc_all.param.cof_off_all[Bs];
	}
	else
		spd_off_all[Xr] = spd_off_all[Xr] * w_ff + (1 - w_ff) * 0;

	if (vmc_all.param.robot_mode == M_C_TROT)
		spd_off_all[Xr] *= -1;

	//mit yaw
	float _yaw_turn_rate = -(vmc_all.att_rate[YAWr] + vmc_all.param.tar_spd_use_rc.z) / 57.3;
	float x_temp = 0;
	float y_temp = vmc[leg].flag_rl * vmc_all.W / 2;
	//    R << c, s, 0, -s, c, 0, 0, 0, 1;//Rot(z).tanspose---dogXP---
	float sy = sin(-_yaw_turn_rate * (st_t[leg] + vmc_all.delay_time[2] + vmc_all.gait_delay_time) / 2);
	float cy = cos(-_yaw_turn_rate * (st_t[leg] + vmc_all.delay_time[2] + vmc_all.gait_delay_time) / 2);

	float yaw_x = (cy*x_temp + sy * y_temp) * 1 - x_temp;
	float yaw_y = (-sy * x_temp + cy * y_temp) * 1 - y_temp;

	END_POS yaw_correct_n, yaw_correct_b;
	yaw_correct_b.x = yaw_x;
	yaw_correct_b.y = yaw_y;
	yaw_correct_b.z = 0;
	converV_b_to_n(yaw_correct_b.x, yaw_correct_b.y, yaw_correct_b.z,
		&yaw_correct_n.x, &yaw_correct_n.y, &yaw_correct_n.z);//转换速度直接输出到IMP  控制频率不够目前？？
	float x_slide = (0.5f*fabs(vmc_all.pos_n.z) / 9.81f)  * (vmc_all.spd_ng.y*_yaw_turn_rate);
	float y_slide = -(0.5f*fabs(vmc_all.pos_n.z) / 9.81f) * (vmc_all.spd_ng.x*_yaw_turn_rate);
	yaw_correct_n.x += x_slide * 2;
	yaw_correct_n.y += y_slide * 2;

	//printf("_yaw_turn_rate=%f y_slide=%f\n", _yaw_turn_rate, y_slide);

#if 1
	rotate_correct[leg].x = yaw_correct_n.x;//MIT
	rotate_correct[leg].y = yaw_correct_n.y;

	//        rotate_correct[leg].x =0;//MIT
	//        rotate_correct[leg].y =0;
#endif
#if 1
	temp_h = fabs(vmc_all.pos_n.z);
	//temp_h = fabs(vmc_all.tar_pos.z);
	spd_off[Xr] = -my_sqrt((temp_h + vmc_all.ankle_pos_n[leg].z * -1*0) / 9.81)*  (tar_spd[Xr] - vmc_all.spd_ng.x);

#else
	temp_h = fabs(vmc_all.tar_pos.z);
	spd_off[Xr] = my_sqrt((temp_h) / 9.81)*  (vmc[leg].spd_hip_n.x - tar_spd[Xr]);
#endif

	tar_epos_slip_ncom.x = LIMIT(
		(//vmc_all.spd_ng.x
			tar_spd[Xr]
			)*
		spd_kk[Xr] * (st_t[leg] + vmc_all.delay_time[2] * 1 + vmc_all.gait_delay_time) / 2 * (1 + 2 * vmc_all.gait_mode == WALK) +
		spd_off[Xr] +
		acc_off[Xr] +
		rotate_correct[leg].x +
		spd_off_all[Xr] * x_off_inv +
		vmc_all.climb_off.x +
		vmc_all.param.sw_com_off[Xr] * x_off_inv
		, MIN_X, MAX_X);

	//###################################################################################
	if ((tar_spd[Yr]) >= 0)
		spd_off_all[Yr] = 0;
	else
		spd_off_all[Yr] = 0;

	temp_h = fabs(vmc_all.pos_n.z);
	//spd_off[Yr] = vmc_all.kp_trig[0] * (tar_spd[Yr] - vmc_all.spd_ng.y)*sqrt((temp_h) / 9.81);
	spd_off[Yr] = -my_sqrt((temp_h + vmc_all.ankle_pos_n[leg].z * -1 * 0) / 9.81)*  (tar_spd[Yr] - vmc_all.spd_ng.y);
	cog_off_all[Yr] = 0;

	tar_epos_slip_ncom.y = LIMIT(
		(
			//vmc_all.spd_ng.y
			tar_spd[Yr]
			)*spd_kk[Yr] * (st_t[leg] + vmc_all.delay_time[2] * 1 + vmc_all.gait_delay_time) / 2 * (1 + 2 * vmc_all.gait_mode == WALK) +
		spd_off[Yr] +
		acc_off[Yr] +
		rotate_correct[leg].y +
		spd_off_all[Yr] +
		cog_off_all[Yr]
		, MIN_Y, MAX_Y);

	//momcassie
	float side_w = 0 * vmc_all.param.param_vmc.side_off[Yr] * sign(tar_spd[Xr]);// 0.06 * 1;//-左右晃  +抑制
	//temp_h=fabs(vmc_all.tar_pos.z);
	if (leg == 1) {//左侧支撑
		mom_lip[Yr].T_c = st_t[leg] + vmc_all.delay_time[2] * 1 + vmc_all.gait_delay_time;
		momentum_planner3(&mom_lip[Yr], temp_h, side_w, dt);
		momentum_estimator3(&mom_lip[Yr], temp_h, dt);
		lateral_controller3(&mom_lip[Yr]);
	}
	else {
		mom_lip[Yr].T_c = st_t[leg] + vmc_all.delay_time[2] * 1 + vmc_all.gait_delay_time;
		momentum_planner3(&mom_lip[Yr], temp_h, side_w, dt);
		momentum_estimator3(&mom_lip[Yr], temp_h, dt);
		lateral_controller3(&mom_lip[Yr]);
	}
#if 0//输出
	//printf("vmc_all.spd_ng.y)=%f\n", vmc_all.spd_ng.y);
	if (fabs(vmc_all.spd_ng.y) < 0.65&&_hu_model.gait_mode != G_DWALK_V && _hu_model.gait_mode != G_PACE_V && vmc_all.gait_mode != WALK) {
		tar_epos_slip_ncom.y = mom_lip[Yr].swing_pos.y + rotate_correct[leg].y;
		//ar_epos_slip_ncom.y += (tar_spd[Yr] * 0 + rotate_off_y)*spd_kk[Yr] * (st_t[leg] + vmc_all.delay_time[2] * 1 + vmc_all.gait_delay_time) / 2;
		 //printf("vmc_all.spd_ng.y=%f side_y=%f slip=%f\n", vmc_all.spd_ng.y, vmc_all.param.param_vmc.side_off[Yr], tar_epos_slip_ncom.y);
	}
#endif

	//##################################################################################
	tar_epos_slip_ncom.z = LIMIT(-fabs(vmc_all.pos_n.z), MAX_Z, MIN_Z);

	//#################################################################################
	lf_time = lf_offz * vmc[i].delta_ht / lf_spd;
	td_time = td_offz * vmc[i].delta_ht / td_spd;
	//printf("%f\n", vmc[i].delta_ht);
	float td_x_off = td_time * LIMIT(tar_spd[Xr], MIN_X * 0.15, MAX_X*0.15);//
	float td_y_off = td_time * LIMIT(tar_spd[Yr], MIN_Y * 0.15, MAX_Y*0.15);//
	i = leg;
	vmc[i].tar_epos.x = tar_epos_slip_ncom.x;
	vmc[i].tar_epos.y = tar_epos_slip_ncom.y;
	vmc[i].tar_epos.z = tar_epos_slip_ncom.z;
	//------------------------------------Xr-------------------------------------
	float leg_off = 0;

	//sin
	vmc[i].tar_epos.x += td_x_off * EN_TD_XOFF;//TD
	vmc[i].tar_epos.x += leg_off;//
	vmc[i].tar_epos.x += robotwb.mess_off[Xr];
	//------------------------------------Yr--------------------------------------
	size_off[Yr] = vmc[i].flag_rl*vmc_all.param.param_vmc.side_off[Yr];//
	vmc[i].tar_epos.y += td_y_off * EN_TD_XOFF;//TD
	vmc[i].tar_epos.y += size_off[Yr] * 1;
	vmc[i].tar_epos.y += robotwb.mess_off[Yr];
	//------------------------------------Zr--------------------------------------
	if (vmc_all.gait_mode == WALK) {
		vmc[i].tar_epos.x -= walk_gait.leg_off_move.x;
		vmc[i].tar_epos.y -= walk_gait.leg_off_move.y;

		vmc[i].tar_epos.x += st_t[leg] * 0.5;
		printf("[%d] %f %f\n", i, walk_gait.leg_off_move.x, walk_gait.leg_off_move.y);
	}
	//###############################################################################
	END_POS ankle_pos_n[4];
	END_POS ankle_pos_b[4];
	END_POS end_epos_n[4];
	END_POS end_epos_b[4];
	END_POS end_epos_n1[4];
	END_POS end_epos_h[4];
	i = leg;
	//Vec3<float> Pf = seResult.position + seResult.rBody.transpose() * (pYawCorrected
	//	+ des_vel * swingTimeRemaining[i]);
	END_POS hip_sw_remain;
	vmc[i].param.swingTimeRemaining -= dt;
	//vmc[i].param.swingTimeRemaining = LIMIT(vmc[i].param.swingTimeRemaining, 0, 99);
	hip_sw_remain.x = tar_spd[Xr] * vmc[i].param.swingTimeRemaining;
	hip_sw_remain.y = tar_spd[Yr] * vmc[i].param.swingTimeRemaining;
	//----------------------------------
	//COM->{b}
	ankle_pos_b[i].x = 0;
	ankle_pos_b[i].y = vmc[i].flag_rl*vmc_all.W / 2;
	ankle_pos_b[i].z = -Hw;

	//{{b}->{n}}
	float RTb_n_target[3][3];//
	float att_rt_use[3];
#if 0//WS 在侧上斜坡不太好
	ground_att[PITr] = vmc_all.ground_att_cmd[PITr] * 1;//落足地形转换
	ground_att[ROLr] = vmc_all.ground_att_cmd[ROLr] * 1;
	att_rt_use[PITr] = vmc_all.att_trig[PITr] - 1 * ground_att[PITr];//(vmc_all.tar_att[PITr]+vmc_all.tar_att_bias[PITr]*1+vmc_all.tar_att_off[PITr]);
	att_rt_use[ROLr] = vmc_all.att_trig[ROLr] - 1 * ground_att[ROLr];//(vmc_all.tar_att[ROLr]+vmc_all.tar_att_bias[ROLr]*1+vmc_all.tar_att_off[ROLr]);
	att_rt_use[YAWr] = 0;

	RTb_n_target[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
	RTb_n_target[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
	RTb_n_target[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

	RTb_n_target[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	RTb_n_target[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	RTb_n_target[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

	RTb_n_target[0][2] = -sind(-att_rt_use[PITr]);
	RTb_n_target[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
	RTb_n_target[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);

	converV_b_to_n_RT(RTb_n_target, 0, ankle_pos_b[i].x, ankle_pos_b[i].y, ankle_pos_b[i].z,
		&ankle_pos_n[i].x, &ankle_pos_n[i].y, &ankle_pos_n[i].z);
#else
	ankle_pos_n[i].x = ankle_pos_b[i].x;
	ankle_pos_n[i].y = ankle_pos_b[i].y;
	ankle_pos_n[i].z = ankle_pos_b[i].z;
#endif
	//
	end_epos_n[i].x = vmc[i].tar_epos.x + ankle_pos_n[i].x + hip_sw_remain.x * 0;
	end_epos_n[i].y = vmc[i].tar_epos.y + ankle_pos_n[i].y + hip_sw_remain.y * 0;
	end_epos_n[i].z = vmc[i].tar_epos.z + ankle_pos_n[i].z*trig_z_flag;//

	//避开碰撞
	END_POS side_leg_n;
	if (i == 0)
		side_leg_n = vmc[1].epos_n;
	if (i == 1)
		side_leg_n = vmc[0].epos_n;


	if ((i == 0) && (end_epos_n[i].y < side_leg_n.y + safe_bind_win[Yr])) {
		//printf("Right Leg [%d] will konck leg \n", i);
		if (fabs(end_epos_n[i].y - side_leg_n.y) < 1.5* safe_bind_win[Yr])
			end_epos_n[i].x += -safe_bind_win[Xr] * vmc[i].flag_fb;
		else
			end_epos_n[i].x += safe_bind_win[Xr] * vmc[i].flag_fb;
	}
	if ((i == 1) && (end_epos_n[i].y > side_leg_n.y - safe_bind_win[Yr])) {
		//printf("Left Leg [%d] will konck leg \n", i);
		if (fabs(end_epos_n[i].y - side_leg_n.y) < 1.5* safe_bind_win[Yr])
			end_epos_n[i].x += -safe_bind_win[Xr] * vmc[i].flag_fb;
		else
			end_epos_n[i].x += safe_bind_win[Xr] * vmc[i].flag_fb;
	}


	float ground_z = (tar_epos_slip_ncom.x)*sind(my_deathzoom_2(vmc_all.ground_att_est[PITr], 0) * 1) +// *sign(tar_spd[Xr]) +
		-(tar_epos_slip_ncom.y)*sind(my_deathzoom_2(vmc_all.ground_att_est[ROLr], 0) * 1);//*sign(tar_spd[Yr]);

//if (ground_z < 0)ground_z = 0;//仅上台阶
//ground_z = 0;
#if EN_SW_SLOP_ETH
	//if(vmc_all.ground_att_est[PITr] * vmc_all.param.tar_spd_use_rc.x > 0)
	  // ground_z = 0;

	sw_z[i] = vmc[i].delta_ht - lf_offz * vmc[i].delta_ht + ground_z;//抬腿高度

	if (trig_z_flag == 99) {
		end_epos_n[i].z = vmc[i].epos_lf_n.z +
			//sind(vmc_all.ground_att_cmd[PITr])*vmc[i].delta_ht*sign(vmc_all.spd_n.x)*1.25
			ground_z * (EN_PLAN_USE_JERK == 5);
		//printf("leg=%d ground_off_x=%f\n", i, ground_z);
		//end_epos_n[i].x += cos(ground_att[PITr])*vmc[i].delta_ht;
	}
#else
	sw_z[i] = vmc[i].delta_ht - lf_offz * vmc[i].delta_ht;// +ground_z / 2;//抬腿高度
	if (trig_z_flag == 99) {
#if 1//上台阶还行
		end_epos_n[i].z = vmc[i].epos_lf_n.z +//使用上次着地
			//sind(vmc_all.ground_att_cmd[PITr])*vmc[i].delta_ht*sign(vmc_all.spd_n.x)*1.25
			ground_z;
#else
		end_epos_n[i].z = vmc_all.ground_plane_p[2]
			+ vmc_all.ground_plane_p[0] * (tar_epos_slip_ncom.y + ankle_pos_b[i].y)//预估地形
			+ vmc_all.ground_plane_p[1] * (tar_epos_slip_ncom.x + ankle_pos_b[i].x)
			+ td_offz * vmc[i].delta_ht * 1;
#endif
		//printf("leg=%d ground_off_x=%f\n", i, ground_z);
		//end_epos_n[i].x += cos(ground_att[PITr])*vmc[i].delta_ht;
		if (vmc[i].param.is_up_stair == 1) {
			end_epos_n[i].z = vmc[i].param.stair_pos.z + td_offz * vmc[i].delta_ht;
			if (vmc[i].delta_ht < fabs(vmc[i].param.stair_pos.z)) {//依据地形调节高度
				sw_z[i] = fabs(vmc[i].param.stair_pos.z);
				if (EN_PLAN_USE_JERK == 5)
					swing_jerk_planner_5point(&vmc[i], lf_spd, td_spd, sw_z[i],
						sw_t[i] - st_t[i] - lf_time - td_time - vmc[i].param.trap_sw_t_fix);//缩短实际
				else
					swing_jerk_planner_3point(&vmc[i], lf_spd, td_spd, sw_z[i],
						sw_t[i] - st_t[i] - lf_time - td_time - vmc[i].param.trap_sw_t_fix);
			}
		}
	}
#endif

	//printf("%f %f\n", cos(ground_att[PITr])*vmc[i].delta_ht,sind(vmc_all.ground_att_cmd[PITr])*vmc[i].delta_ht*sign(tar_spd[Xr]));
		//end_epos_n[i].z = vmc[i].epos_lf_n.z+td_offz*vmc[i].delta_ht;//默认地形水平假设
	//--------------------- {n}------------------
	//-------------------------------------
//		end_epos_n1[i].x=end_epos_n[i].x-ankle_pos_n[i].x;
//		end_epos_n1[i].y=end_epos_n[i].y-ankle_pos_n[i].y;
//		end_epos_n1[i].z=end_epos_n[i].z;

	if (ocu.key_y && !ocu.key_y_reg)
		leg_lock_n_trigt[0] = !leg_lock_n_trigt[0];
	leg_lock_n_trigt[0] = leg_lock_n_trigt[2] = ocu.key_y;//doghome
	//printf("leg_lock_n_trigt[0]=%d\n",leg_lock_n_trigt[0]);
	if (vmc_all.gait_mode == TROT || vmc_all.gait_mode == F_TROT) {
		leg_lock_n_trigt[2] = leg_lock_n_trigt[0];
		leg_lock_n_trigt[3] = leg_lock_n_trigt[1];
	}

	if (leg_lock_n_trigt[i] && !leg_lock_n_trig_regt[i])//
		end_lock_nt[i] = vmc[i].epos_nn;

	if (leg_lock_n_trigt[i]) {//
		end_epos_gn[i] = end_lock_nt[i];
	}
	leg_lock_n_trig_regt[i] = leg_lock_n_trigt[i];


	end_epos_n1[i].x = end_epos_gn[i].x - vmc_all.cog_pos_n.x;
	end_epos_n1[i].y = end_epos_gn[i].y - vmc_all.cog_pos_n.y;
	end_epos_n1[i].z = end_epos_gn[i].z;

	if (leg_lock_n_trigt[i]) {
		end_epos_n[i].x = end_epos_n1[i].x;
		end_epos_n[i].y = end_epos_n1[i].y;
		//end_epos_n[i].z=end_epos_n1[i].z;//
	}

	float slip_flt = 0.05;
	if (vmc[i].slip_epos_n_flt_init) {
		vmc[i].slip_epos_n_flt_init = 0;
		vmc[i].slip_epos_n = end_epos_n[i];//内部slip模型点
	}
	vmc[i].slip_epos_n.x = end_epos_n[i].x*slip_flt + (1 - slip_flt)*vmc[i].slip_epos_n.x;//内部slip模型点
	vmc[i].slip_epos_n.y = end_epos_n[i].y*slip_flt + (1 - slip_flt)*vmc[i].slip_epos_n.y;
	vmc[i].slip_epos_n.z = end_epos_n[i].z*slip_flt + (1 - slip_flt)*vmc[i].slip_epos_n.z;

	static END_POS location_visual[4];
	int good_location = 0;
#if EN_HUMAN
#if HU_TASK==HU_TASK2
	if ((vmc[i].param.time_trig < 0.5* (sw_t[i] - st_t[i] - lf_time - td_time) || 0) && 1)
		good_location = location_select_on_map(i, vmc[i].slip_epos_n, 0, &location_visual[i]);
#elif HU_TASK==HU_TASK4
	if ((vmc[i].param.time_trig < 0.45* (sw_t[i] - st_t[i] - lf_time - td_time) || 0) && 1)
		good_location = location_select_on_map(i, vmc[i].slip_epos_n, 0, &location_visual[i]);
#endif
	if ((vmc[i].param.time_trig < 0.5* (sw_t[i] - st_t[i] - lf_time - td_time) || 0) && _hu_model.fake_map_guass)
		good_location = location_select_on_map(i, vmc[i].slip_epos_n, 0, &location_visual[i]);
#endif
	//-------------------- --------------------
	if ((good_location || vision_sw_enable[i]) && EN_VISION&&EN_HUMAN)
	{
		vision_sw_enable[i] = 1;
		vmc[i].param.vision_sw_enable = 1;
		vmc[i].tar_epos_n.x = location_visual[i].x;
		vmc[i].tar_epos_n.y = location_visual[i].y;
		vmc[i].tar_epos_n.z = -fabs(vmc_all.pos_n.z) + location_visual[i].z + td_offz * vmc[i].delta_ht - 0.04*(HU_TASK == HU_TASK1);
		//vmc[i].tar_epos_n.z = -fabs(vmc_all.pos_n.z) + location_visual[i].z+td_offz * vmc[i].delta_ht;
		if (vmc[i].delta_ht < fabs(location_visual[i].z)) {//依据地形调节高度
			sw_z[i] = location_visual[i].z;
			if (EN_PLAN_USE_JERK == 5)
				swing_jerk_planner_5point(&vmc[i], lf_spd, td_spd, sw_z[i],
					sw_t[i] - st_t[i] - lf_time - td_time - vmc[i].param.trap_sw_t_fix);//缩短实际
			else
				swing_jerk_planner_3point(&vmc[i], lf_spd, td_spd, sw_z[i],
					sw_t[i] - st_t[i] - lf_time - td_time - vmc[i].param.trap_sw_t_fix);

		}
	}
	else {
		vmc[i].tar_epos_n.x = end_epos_n[i].x;
		vmc[i].tar_epos_n.y = end_epos_n[i].y;
		if (!recal) {
			vmc[i].tar_epos_n.z = end_epos_n[i].z;
			//vmc[i].tar_epos_n.z += td_offz;
		}
	}
}


float end_pos_y_correction(VMC* in, float ep_slip_n, float dt)//
{
	END_POS td_vir_n, lo_vir_n;
	float end_pos_y_comn = 0;
	float end_pos_y_n = 0;
	float fb_correction = 0;
	float roll_correction = 0;
	float force_correction = 0;
	float cmd_correction = 0;
	static float err_v_interge_correction = 0;
	cal_vir_leg_pos(in, &td_vir_n, &lo_vir_n);//COM
	fb_correction = pd_y[0] * (td_vir_n.y - lo_vir_n.y) + pd_y[1] * (td_vir_n.y + lo_vir_n.y);//

	force_correction = 0;//

	err_v_interge_correction = 0;

	cmd_correction = -vmc_all.param.tar_spd_use_rc.y*kp_rc;

	end_pos_y_comn =
		-fb_correction
		+ cmd_correction//
		+ force_correction
		+ err_v_interge_correction
		+ roll_correction;

	end_pos_y_comn = LIMIT(end_pos_y_comn, MIN_Y*0.35, MAX_Y*0.35);//

	//-------------------------------------------------
	END_POS ankle_pos_n;
	ankle_pos_n.x = ankle_pos_n.y = ankle_pos_n.z = 0;
	END_POS ankle_pos_b;
	ankle_pos_b.x = ankle_pos_b.y = ankle_pos_b.z = 0;
	ankle_pos_b.x = in->flag_fb*vmc_all.H / 2;
	ankle_pos_b.y = in->flag_rl*vmc_all.W / 2;
	ankle_pos_b.z = 0;

	converV_b_to_n(ankle_pos_n.x, ankle_pos_n.y, 0,
		&ankle_pos_n.x, &ankle_pos_n.y, &ankle_pos_n.z);

	end_pos_y_n = ep_slip_n +//
		end_pos_y_comn;//


//----------------------------------------------


//----------------------------------------
	return (end_pos_y_n - in->epos_n.y) / dt;
}

void trig_curve_global(VMC *in, float *x, float *y, float *z, float *vx, float *vy, float *vz, float dt)//
{
	END_POS epos, epos_nn;
	int id = in->param.id, temp = 0;
	//printf("%f %f\n",sw_t[i],st_t[i]);
	in->param.time_trig += dt;
	if (in->param.time_trig < sw_t[id] - st_t[id] - td_time - lf_time)
	{
#if EN_PLAN_USE_JERK==0
#if EN_PLAN_USE_MIT
		get_swing_Bezier(in,
			sw_t[id] - st_t[id] - td_time - lf_time,
			in->param.time_trig);
		*x = in->param.end_planner.pos_now[Xr];
		*y = in->param.end_planner.pos_now[Yr];
		*z = LIMIT(in->param.end_planner.pos_now[Zr],
			MAX_Z + in->flag_fb*vmc_all.H / 2 * sind(vmc_all.ground_att_est[PITr]),
			MIN_Z + in->flag_fb*vmc_all.H / 2 * sind(vmc_all.ground_att_est[PITr]));
		*vx = in->param.end_planner.spd_now[Xr];
		*vy = in->param.end_planner.spd_now[Yr];
		*vz = in->param.end_planner.spd_now[Zr];
#else
		epos = cal_pos_tar_from_curve(in, sw_t[i] - st_t[i] - td_time - lf_time, dt);
		*x = epos.x;
		*y = epos.y;
		*z = epos.z;//LIMIT(epos.z, MAX_Z, MIN_Z);
#endif
#else
		foot_trap_detector(in, 2, dt);//采集

		if (EN_PLAN_USE_JERK == 5)
			get_swing_jerk_5point(in, in->param.time_trig);
		else
			get_swing_jerk_3point(in, in->param.time_trig);

		*x = in->param.end_planner.pos_now[Xr];
		*y = in->param.end_planner.pos_now[Yr];
		*z = LIMIT(in->param.end_planner.pos_now[Zr],
			MAX_Z,
			MIN_Z);

		*vx = in->param.end_planner.spd_now[Xr];
		*vy = in->param.end_planner.spd_now[Yr];
		*vz = in->param.end_planner.spd_now[Zr];
#endif
	}
}


void espd_to_qspd(VMC *in, END_POS spd_end, float* spd_q, float dt)//
{
	float temp[3] = { 0,0,0 };
	float spd_cmd[3];
	spd_cmd[Xr] = spd_end.x;
	spd_cmd[Yr] = spd_end.y;
	spd_cmd[Zr] = spd_end.z;

	temp[D_LEG] = in->ijacobi33[0] * spd_cmd[Xr] * in->param.invert_knee_epos[Xr] +
		in->ijacobi33[1] * spd_cmd[Yr] * in->param.invert_knee_epos[Yr] +
		in->ijacobi33[2] * spd_cmd[Zr];
	temp[X_LEG] = in->ijacobi33[3] * spd_cmd[Xr] * in->param.invert_knee_epos[Xr] +
		in->ijacobi33[4] * spd_cmd[Yr] * in->param.invert_knee_epos[Yr] +
		in->ijacobi33[5] * spd_cmd[Zr];
	temp[T_LEG] = in->ijacobi33[6] * spd_cmd[Xr] * in->param.invert_knee_epos[Xr] +
		in->ijacobi33[7] * spd_cmd[Yr] * in->param.invert_knee_epos[Yr] +
		in->ijacobi33[8] * spd_cmd[Zr];


#if 0
	spd_q[D_LEG] = temp[D_LEG] * in->param.flt_toqrue + (1 - in->param.flt_toqrue)*spd_q[D_LEG];
	spd_q[X_LEG] = temp[X_LEG] * in->param.flt_toqrue + (1 - in->param.flt_toqrue)*spd_q[X_LEG];
	spd_q[T_LEG] = temp[T_LEG] * in->param.flt_toqrue + (1 - in->param.flt_toqrue)*spd_q[T_LEG];
#else
	DigitalLPF(temp[D_LEG], &spd_q[0], FLT_SW_END, dt);
	DigitalLPF(temp[X_LEG], &spd_q[1], FLT_SW_END, dt);
	DigitalLPF(temp[T_LEG], &spd_q[2], FLT_SW_END, dt);
#endif
}


void reset_sw_torque_as_now(VMC *in, float torque_now[3])
{
	in->param.spd_dj[D_LEG] = torque_now[D_LEG];
	in->param.spd_dj[X_LEG] = torque_now[X_LEG];
	in->param.spd_dj[T_LEG] = torque_now[T_LEG];
}


void trig_plan(char leg_sel_trig, float dt) {

#if RUN_PI
	lf_spd_trot = config_gait["leg_planner_param"]["lf_spd_trot"].as<float>();// 0.8;
	td_spd_trot = config_gait["leg_planner_param"]["td_spd_trot"].as<float>();//0.8;

	lf_offz = config_gait["leg_planner_param"]["lf_offz"].as<float>();//0.0;
	td_offz = config_gait["leg_planner_param"]["td_offz"].as<float>();//0.1;
#endif

	sw_t[leg_sel_trig] = vmc_all.gait_time[1];
	st_t[leg_sel_trig] = vmc_all.stance_time;
	vmc[leg_sel_trig].param.swingTimeRemaining = sw_t[leg_sel_trig] - st_t[leg_sel_trig];

	if (leg_sel_trig == 1) {//左侧支撑
		momentum_control_init3(&mom_lip[Yr], Ls, st_t[leg_sel_trig]);
		//mom_lip[Yr].contact_pos = vmc_all.pos_nb_kf;
		mom_lip[Yr].contact_pos.y += vmc[1].epos_n.y;//接触点偏差  加上后会交叉腿  不加没法内收
	}
	else {
		momentum_control_init3(&mom_lip[Yr], Rs, st_t[leg_sel_trig]);
		//mom_lip[Yr].contact_pos = vmc_all.pos_nb_kf;
		mom_lip[Yr].contact_pos.y += vmc[0].epos_n.y;//接触点偏差
	}

	vision_sw_enable[leg_sel_trig] = 0;
	vmc[leg_sel_trig].param.vision_sw_enable = 0;
	vmc[leg_sel_trig].param.trap_sw_t_fix = 0;//清除时间
	vmc[leg_sel_trig].epos_lf_n = vmc[leg_sel_trig].epos_n;//
	vmc[leg_sel_trig].epos_lf_hn = vmc[leg_sel_trig].eposc;
	vmc[leg_sel_trig].slip_epos_n_flt_init = 1;
	sw_z[leg_sel_trig] = vmc[leg_sel_trig].delta_ht - lf_offz * vmc[leg_sel_trig].delta_ht;//抬腿高度
	cal_tar_end_pos_n_v1_sel(leg_sel_trig, 0, dt);//
	vmc[leg_sel_trig].cnt_ss = 0;

	foot_trap_detector(&vmc[leg_sel_trig], 1, dt);//复位
	upstair_detector(&vmc[leg_sel_trig], 1, dt);
	if (vmc_all.gait_mode == WALK)
	{
		lf_spd = lf_spd_walk;
		td_spd = td_spd_walk;
		EN_LIFT_Y_CONTROL = 1;
		EN_TD_Y_CONTROL = 1;
	}
	else
	{
		lf_spd = lf_spd_trot;
		td_spd = td_spd_trot;
		EN_LIFT_Y_CONTROL = 1;
		EN_TD_Y_CONTROL = 1;
	}
	vmc[leg_sel_trig].ground = 0;
	stand_force_enable_flag[leg_sel_trig] = 0;

	float sita_side1 = atan(vmc_all.param.param_vmc.side_off[Yr] / vmc_all.pos_n.z)*57.3;
	float l = sqrt(pow(vmc_all.param.param_vmc.side_off[Yr], 2) + pow(vmc_all.pos_n.z, 2));
	float sita_side2 = asin(vmc[leg_sel_trig].l3 / l)*57.3;
	//printf("sita_side=%f sita_side2=%f\n", sita_side1, sita_side2);
	vmc[leg_sel_trig].param.y_side = tand(fabs(sita_side2 - sita_side1)*vmc[leg_sel_trig].flag_rl)*sw_z[leg_sel_trig]*0;//内收造成的偏距
}

char trig_lift(char leg_sel_trig, float dt) {
	Vect3 temp_g, temp_n;
	vmc[leg_sel_trig].ground = 0;
	stand_force_enable_flag[leg_sel_trig] = 0;

	vmc[leg_sel_trig].epos_spdd_n.x = LIMIT(-vmc_all.spd_n.x, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Xr];//
	vmc[leg_sel_trig].epos_spdd_n.y = LIMIT(-vmc_all.spd_n.y, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Yr];//

	vmc[leg_sel_trig].epos_spdd_n.z = lf_spd;
	temp_n.x = vmc[leg_sel_trig].epos_spdd_n.x;
	temp_n.y = vmc[leg_sel_trig].epos_spdd_n.y;
	temp_n.z = vmc[leg_sel_trig].epos_spdd_n.z;
	converV_n_to_g(temp_n, &temp_g);
	vmc[leg_sel_trig].epos_spdd_n.x = temp_g.x;
	vmc[leg_sel_trig].epos_spdd_n.y = temp_g.y;

	//if (EN_SLIP_CONTROL&&EN_LIFT_Y_CONTROL)
	//	vmc[leg_sel_trig].epos_spdd_n.y = end_pos_y_correction(&vmc[leg_sel_trig], vmc[leg_sel_trig].epos_lf_n.y, dt);//
	//else
	//	vmc[leg_sel_trig].epos_spdd_n.y = LIMIT(vmc_all.spd_n.y, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Yr];

	if (vmc_all.param.leg_dof == 2)
		vmc[leg_sel_trig].epos_spdd_n.y = 0;
	vmc[leg_sel_trig].cnt_ss += dt;
	//if(leg_sel_trig==3)
	printf("LF %d: %f %f\n", leg_sel_trig,vmc[leg_sel_trig].epos_spdd_n.x,vmc[leg_sel_trig].epos_n.x);
	//printf("%d %d\n", vmc[0].ground, vmc[1].ground);
	if (fabs(vmc[leg_sel_trig].epos_n.z - vmc[leg_sel_trig].epos_lf_n.z) >= lf_offz * vmc[leg_sel_trig].delta_ht
		// || vmc[diag_leg_id].param.trig_state == 2
		|| vmc[leg_sel_trig].cnt_ss >= lf_time
		) {//
		vmc[leg_sel_trig].st_pos = vmc[leg_sel_trig].epos_sw_st_n = vmc[leg_sel_trig].epos_n;//起点
		vmc[leg_sel_trig].st_spd = vmc[leg_sel_trig].spd_n;
		if (leg_sel_trig == 3)
			printf("vmc[%d].spd_n=%f %f\n", leg_sel_trig, vmc[leg_sel_trig].spd_n.x, vmc[leg_sel_trig].spd_n.z);
		vmc[leg_sel_trig].tar_pos = vmc[leg_sel_trig].tar_epos_n;//
		vmc[leg_sel_trig].param.time_trig = dt;
		vmc[leg_sel_trig].param.ground_state = vmc[leg_sel_trig].param.ground_state = 0;

		//        reset_tar_pos(leg_sel_trig);

		vmc[leg_sel_trig].param.tar_epos_n_reg = vmc[leg_sel_trig].param.tar_epos_n;
#if EN_PLAN_USE_JERK
		if (EN_PLAN_USE_JERK == 5)
			swing_jerk_planner_5point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
		else
			swing_jerk_planner_3point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
#else
#if EN_PLAN_USE_MIT
		swing_Bezier_planner(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
#else
		cal_curve_from_pos_new(&vmc[leg_sel_trig], vmc[leg_sel_trig].tar_pos, sw_z[leg_sel_trig], sw_t[i] - st_t[i] - lf_time);
#endif
#endif
		vmc[leg_sel_trig].cnt_ss = 0;
		vmc[leg_sel_trig].param.tar_epos_n_reg = vmc[leg_sel_trig].param.tar_epos_n;
		cnt_online_plan[leg_sel_trig] = 0;
		return 1;
	}
	else
		return 0;
}

char trig_swing(char leg_sel_trig, float dt) {
	END_POS end_spd_n;
	end_spd_n.x = end_spd_n.y = end_spd_n.z = 0;

#if EN_ONLINE_SW_PLAN
	cnt_online_plan[leg_sel_trig] += dt;
	if (cnt_online_plan[leg_sel_trig] >= RE_PLAN_DT) {
		trig_plan_online(leg_sel_trig, cnt_online_plan[leg_sel_trig]);
		cnt_online_plan[leg_sel_trig] = 0;
	}
#endif

	trig_curve_global(&vmc[leg_sel_trig],
		&vmc[leg_sel_trig].param.tar_epos_n.x,
		&vmc[leg_sel_trig].param.tar_epos_n.y,
		&vmc[leg_sel_trig].param.tar_epos_n.z,
		&end_spd_n.x,
		&end_spd_n.y,
		&end_spd_n.z,
		dt);

	webots_draw_planning_traj(&vmc[leg_sel_trig], dt);

#if EN_SW_SLOP_ETH&&EN_PLAN_USE_JERK!=5//转地形坐标ETH
	float dx = vmc[leg_sel_trig].param.tar_epos_n.x - vmc[leg_sel_trig].st_pos.x;
	float dz = vmc[leg_sel_trig].param.tar_epos_n.z - vmc[leg_sel_trig].st_pos.z;

	float r_dx, r_dz;
	float g_att = my_deathzoom_2(vmc_all.ground_att_est[PITr] + FIX_SLOP_OFF, SLOP_DEAD);
	r_dx = cosd(g_att)*dx - sind(g_att)*dz;
	r_dz = sind(g_att)*dx + cosd(g_att)*dz;
	if ((g_att > 0 && vmc_all.param.tar_spd_use_rc.x > 0) || 0) {
		vmc[leg_sel_trig].param.tar_epos_n.x = vmc[leg_sel_trig].st_pos.x + r_dx;
		vmc[leg_sel_trig].param.tar_epos_n.z = vmc[leg_sel_trig].st_pos.x + r_dz;
	}
#endif
	//if(leg_sel_trig==0)
	 //   printf("%f %f %f\n",vmc[leg_sel_trig].param.tar_epos_n.x,vmc[leg_sel_trig].param.tar_epos_n.y,vmc[leg_sel_trig].param.tar_epos_n.z);
#if EN_PLAN_USE_JERK==5&&0
	vmc[leg_sel_trig].epos_spdd_n.x = end_spd_n.x;
	vmc[leg_sel_trig].epos_spdd_n.y = end_spd_n.y;
	vmc[leg_sel_trig].epos_spdd_n.z = end_spd_n.z;
#else
	vmc[leg_sel_trig].epos_spdd_n.x = end_spd_n.x*0.05 + 0.95* (vmc[leg_sel_trig].param.tar_epos_n.x - vmc[leg_sel_trig].param.tar_epos_n_reg.x) / dt;
	vmc[leg_sel_trig].epos_spdd_n.y = end_spd_n.y*0.05 + 0.95*(vmc[leg_sel_trig].param.tar_epos_n.y - vmc[leg_sel_trig].param.tar_epos_n_reg.y) / dt;
	vmc[leg_sel_trig].epos_spdd_n.z = end_spd_n.z*0.05 + 0.95* (vmc[leg_sel_trig].param.tar_epos_n.z - vmc[leg_sel_trig].param.tar_epos_n_reg.z) / dt;
#endif
	vmc[leg_sel_trig].param.tar_epos_n_reg = vmc[leg_sel_trig].param.tar_epos_n;


	//if (EN_SLIP_CONTROL)
	//	vmc[leg_sel_trig].epos_spdd_n.y = end_pos_y_correction(&vmc[leg_sel_trig], vmc[leg_sel_trig].param.tar_epos_n.y, dt);//
	//else
	//	vmc[leg_sel_trig].epos_spdd_n.y = end_spd_n.y;

	if (vmc_all.param.leg_dof == 2)
		vmc[leg_sel_trig].epos_spdd_n.y = 0;

	if (//leg_sel_trig==2&&
		_hu_model.hu_mode > 0 &&
		vmc_all.param.is_trap == 0 &&//每次只能一条腿
		vmc[leg_sel_trig].param.is_trap == 0 &&
		vmc[leg_sel_trig].param.time_trig > (sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time)*0.47&&
		vmc[leg_sel_trig].param.time_trig < (sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time)*0.51) {//摆动到一半就处理FootTrap
		vmc[leg_sel_trig].param.is_trap = foot_trap_detector(&vmc[leg_sel_trig], 3, dt);//处理
		if (vmc[leg_sel_trig].param.is_trap) {
			vmc_all.param.is_trap = 1;
			vmc[leg_sel_trig].st_pos = vmc[leg_sel_trig].epos_sw_st_n = vmc[leg_sel_trig].epos_n;//起点
			vmc[leg_sel_trig].st_spd = vmc[leg_sel_trig].spd_n;
			vmc[leg_sel_trig].tar_pos = vmc[leg_sel_trig].tar_epos_n;//
			vmc[leg_sel_trig].param.trap_sw_t_fix = vmc[leg_sel_trig].param.time_trig*0.5;
			vmc[leg_sel_trig].param.ground_state = vmc[leg_sel_trig].param.ground_state = 0;

			vmc[leg_sel_trig].param.tar_epos_n_reg = vmc[leg_sel_trig].param.tar_epos_n;
#if EN_PLAN_USE_JERK
			if (EN_PLAN_USE_JERK == 5)
				swing_jerk_planner_5point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig],
					sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time - vmc[leg_sel_trig].param.trap_sw_t_fix);//缩短实际
			else
				swing_jerk_planner_3point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig],
					sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time - vmc[leg_sel_trig].param.trap_sw_t_fix);
#else
#if EN_PLAN_USE_MIT
			swing_Bezier_planner(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
#else
			cal_curve_from_pos_new(&vmc[leg_sel_trig], vmc[leg_sel_trig].tar_pos, sw_z[leg_sel_trig], sw_t[i] - st_t[i] - lf_time);
#endif
#endif
			vmc[leg_sel_trig].param.time_trig = dt;
			vmc[leg_sel_trig].cnt_ss = 0;
			vmc[leg_sel_trig].param.tar_epos_n_reg = vmc[leg_sel_trig].param.tar_epos_n;
			cnt_online_plan[leg_sel_trig] = 0;
			printf("Foot trap replan leg[%d]\n", leg_sel_trig);
		}
	}//--end foot trap
	//if (leg_sel_trig == 2)
	//	printf("t_sw=%f\n", vmc[leg_sel_trig].param.time_trig);

	if (vmc[leg_sel_trig].param.time_trig >=
		(sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time
			- vmc[leg_sel_trig].param.trap_sw_t_fix*vmc[leg_sel_trig].param.is_trap//补偿时间
			)*SW_TD_CHECK_RATE//SW_LOW_RATE
		&& (vmc_all.use_ground_sensor_new == 1 && vmc[leg_sel_trig].is_touch)) {
		vmc[leg_sel_trig].cnt_ss = 0;
		vmc[leg_sel_trig].epos_spdd_n.x = vmc[leg_sel_trig].epos_spdd_n.y = vmc[leg_sel_trig].epos_spdd_n.z = 0;
		vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;
		vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
		vmc[leg_sel_trig].param.time_trig = 0;
		//vmc[leg_sel_trig].param.is_trap = foot_trap_detector(&vmc[leg_sel_trig], 3, dt);//处理
#if !GROUND_AFTER_TRIG
		vmc[leg_sel_trig].ground = 1;//
		stand_force_enable_flag[leg_sel_trig] = 1;
		reset_tar_pos(leg_sel_trig);
#endif
		//printf("SW->END::[%d]\n", leg_sel_trig);
		return 2;
	}

	if (vmc[leg_sel_trig].param.time_trig >=
		sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time
		- vmc[leg_sel_trig].param.trap_sw_t_fix*vmc[leg_sel_trig].param.is_trap//补偿时间
		//|| (vmc[leg_sel_trig].epos_n.z< vmc[leg_sel_trig].tar_epos_n.z&&vmc[leg_sel_trig].param.time_trig >SW_TD_CHECK_RATE_S*(sw_t[i] - st_t[i] - lf_time - td_time))
		)//new)//摆动结束 切换TD
	{
		vmc[leg_sel_trig].param.time_trig = dt;
		vmc[leg_sel_trig].cnt_ss = dt;
		vmc[leg_sel_trig].epos_sw_end_n = vmc[leg_sel_trig].epos_n;
		//printf("SW->TD::[%d]\n", leg_sel_trig);
		return 1;
	}
	return 0;
}


char trig_td(char leg_sel_trig, float dt) {
	Vect3 temp_n, temp_g;
	vmc[leg_sel_trig].cnt_ss += dt;
	if (vmc_all.gait_mode == BOUND) {
		vmc[leg_sel_trig].epos_spdd_n.x = 0;//LIMIT(-vmc_all.spd_n.x, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Xr];//
		vmc[leg_sel_trig].epos_spdd_n.z = 0;
	}
	else {
		if (vision_sw_enable[leg_sel_trig] || vmc_all.gait_mode == G_ETL || vmc_all.gait_mode == WALK || HU_TASK == HU_TASK4) {//抑制运动
			vmc[leg_sel_trig].epos_spdd_n.x = -1 * LIMIT(vmc_all.spd_n.x, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Xr];
			vmc[leg_sel_trig].epos_spdd_n.y = -1 * LIMIT(vmc_all.spd_n.y, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Yr];
		}
		else
		{
			vmc[leg_sel_trig].epos_spdd_n.x = -1 * LIMIT(vmc_all.spd_n.x, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Xr];
			vmc[leg_sel_trig].epos_spdd_n.y = -1 * LIMIT(vmc_all.spd_n.y, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Yr];
		}
		vmc[leg_sel_trig].epos_spdd_n.z = -td_spd * (1 + LIMIT(my_deathzoom_2(vmc[leg_sel_trig].cnt_ss*1.25 / SW_TD_OVER_TIME, 0.5), 0, 1));
	}
	temp_n.x = vmc[leg_sel_trig].epos_spdd_n.x;
	temp_n.y = vmc[leg_sel_trig].epos_spdd_n.y;
	temp_n.z = vmc[leg_sel_trig].epos_spdd_n.z;
	converV_n_to_g(temp_n, &temp_g);
	temp_g = temp_n;
	vmc[leg_sel_trig].epos_spdd_n.x = temp_g.x;
	vmc[leg_sel_trig].epos_spdd_n.y = temp_g.y;

#if EN_SW_SLOP_ETH&&0//转地形坐标ETH
	float dx = vmc[leg_sel_trig].epos_spdd_n.x;
	float dz = vmc[leg_sel_trig].epos_spdd_n.z;

	float r_dx, r_dz;
	float g_att = my_deathzoom_2(vmc_all.ground_att_est[PITr] + FIX_SLOP_OFF, SLOP_DEAD);

	if (g_att > 0 && vmc_all.param.tar_spd_use_rc.x > 0 || 0) {
		//printf("vmc_all.ground_att_est[PITr]=%f\n", vmc_all.ground_att_est[PITr]);
		vmc[leg_sel_trig].epos_spdd_n.x = cosd(g_att)*dx - sind(g_att)*dz;
		vmc[leg_sel_trig].epos_spdd_n.z = sind(g_att)*dx + cosd(g_att)*dz;
	}
#endif

	//if (EN_SLIP_CONTROL&&EN_TD_Y_CONTROL)
	//	vmc[leg_sel_trig].epos_spdd_n.y = end_pos_y_correction(&vmc[leg_sel_trig], vmc[leg_sel_trig].epos_sw_end_n.y, dt);//
	//else
	//	vmc[leg_sel_trig].epos_spdd_n.y = LIMIT(vmc_all.spd_n.y, -max_end_spd, max_end_spd)*EN_SPDN_TRIG[Yr];

	if (vmc_all.param.leg_dof == 2)
		vmc[leg_sel_trig].epos_spdd_n.y = 0;
	//printf("TD: %f %f\n",vmc[leg_sel_trig].epos_spdd_n.x,vmc[leg_sel_trig].epos_n.x);
	if (fabs(vmc[leg_sel_trig].epos_n.z) >= fabs(vmc[leg_sel_trig].epos_sw_end_n.z) + td_offz) {
		if (vmc_all.use_ground_sensor_new == 0 || vmc_all.trot_air_test) {
			vmc[leg_sel_trig].cnt_ss = 0;
			vmc[leg_sel_trig].epos_spdd_n.x = vmc[leg_sel_trig].epos_spdd_n.y = vmc[leg_sel_trig].epos_spdd_n.z = 0;
			vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;//��¼����λ��
			vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
			vmc[leg_sel_trig].param.time_trig = 0;
#if !GROUND_AFTER_TRIG
			vmc[leg_sel_trig].ground = 1;//δʹ���ŵش�����Ĭ�Ͻ���
			stand_force_enable_flag[leg_sel_trig] = 1;
			reset_tar_pos(leg_sel_trig);
#endif
			return 1;//�л�֧��
		}//高度着地
	}

	if (vmc[leg_sel_trig].cnt_ss > st_t[leg_sel_trig] * 0.25&&vmc_all.use_ground_sensor_new == 0)//超时 不使用传感器
	{
		vmc[leg_sel_trig].cnt_ss = 0;
		vmc[leg_sel_trig].epos_spdd_n.x = vmc[leg_sel_trig].epos_spdd_n.y = vmc[leg_sel_trig].epos_spdd_n.z = 0;
		vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;//
		vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
		vmc[leg_sel_trig].param.time_trig = 0;
#if !GROUND_AFTER_TRIG
		vmc[leg_sel_trig].ground = 1;//
		stand_force_enable_flag[leg_sel_trig] = 1;
		reset_tar_pos(leg_sel_trig);
#endif
		return 1;//
	}
	//-------------------------------------------------使用传感器----------------------------------------------------------
	if ((vmc_all.use_ground_sensor_new == 1 && vmc[leg_sel_trig].is_touch)) {//着地
		vmc[leg_sel_trig].cnt_ss = 0;
		vmc[leg_sel_trig].epos_spdd_n.x = vmc[leg_sel_trig].epos_spdd_n.y = vmc[leg_sel_trig].epos_spdd_n.z = 0;
		vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;
		vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
		vmc[leg_sel_trig].param.time_trig = 0;

		//vmc[leg_sel_trig].param.is_trap = foot_trap_detector(&vmc[leg_sel_trig], 3, dt);//处理
#if !GROUND_AFTER_TRIG
		vmc[leg_sel_trig].ground = 1;//
		stand_force_enable_flag[leg_sel_trig] = 1;
		reset_tar_pos(leg_sel_trig);
#endif
		//printf("TD->END::[%d]\n",leg_sel_trig);
		//printf("TD End Leg=%d\n", leg_sel_trig);
		return 1;
	}


	if (vmc[leg_sel_trig].cnt_ss > SW_TD_OVER_TIME)//超时
	{
		vmc[leg_sel_trig].cnt_ss = 0;
		vmc[leg_sel_trig].epos_spdd_n.x = vmc[leg_sel_trig].epos_spdd_n.y = vmc[leg_sel_trig].epos_spdd_n.z = 0;
		vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;//
		vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
		vmc[leg_sel_trig].param.time_trig = 0;
#if !GROUND_AFTER_TRIG
		vmc[leg_sel_trig].ground = 1;//
		stand_force_enable_flag[leg_sel_trig] = 1;
		reset_tar_pos(leg_sel_trig);
#endif
		printf("TD->OT::[%d]\n", leg_sel_trig);
		return 1;//
	}

	char diag_leg_id = 0;//对角判断
	/*2    0
	  3    1*/
	switch (leg_sel_trig)
	{
	case 0:
		diag_leg_id = 3;
		break;
	case 1:
		diag_leg_id = 2;
		break;
	case 2:
		diag_leg_id = 1;
		break;
	case 3:
		diag_leg_id = 0;
		break;
	}

	if (
		0 &&
		(fabs(vmc[leg_sel_trig].epos.z) >= fabs(MAX_Z)*0.95
			//||fabs(vmc[leg_sel_trig].epos.x) >= fabs(MAX_X)*0.95
			//||fabs(vmc[leg_sel_trig].epos.z) <= fabs(MIN_Z)*1.05
			)) {
		vmc[leg_sel_trig].epos_spdd_n.z = 0;

		if (vmc[diag_leg_id].is_touch) {
			vmc[leg_sel_trig].epos_spdd_n.x = 0;
			vmc[leg_sel_trig].epos_spdd_n.y = 0;
			vmc[leg_sel_trig].epos_td_n = vmc[leg_sel_trig].epos_n;//
			vmc[leg_sel_trig].cnt_ss = 0;
			vmc[leg_sel_trig].epos_td_hn = vmc[leg_sel_trig].eposc;
			vmc[leg_sel_trig].param.time_trig = 0;
#if !GROUND_AFTER_TRIG
			vmc[leg_sel_trig].ground = 1;//
			stand_force_enable_flag[leg_sel_trig] = 1;
			reset_tar_pos(leg_sel_trig);
#endif
			return 1;//
		}
	}
	return 0;
}

//---------------------------------------------------通用算法

//---------------------------------------------------通用算法
void swing_spd_control(char leg_sel_trig, float dt)//速度输出
{
	//for climb
	//if(vmc[leg_sel_trig].param.time_trig <0.3*( sw_t[i] - st_t[i]-lf_time-td_time))
	// vmc[leg_sel_trig].epos_spdd_n.x-= sind(my_deathzoom_2(fabs(vmc_all.ground_att_cmd[PITr]),0)*2)*sign(tar_spd[Xr]);
#if EN_SW_SLOP_ETH&&0//转地形坐标ETH
	float dx = 0;
	float dz = vmc_all.spd_n.z;

	float r_dx, r_dz;
	float g_att = vmc_all.ground_att_est[PITr] + FIX_SLOP_OFF;

	if (vmc_all.ground_att_est[PITr] * vmc_all.param.tar_spd_use_rc.x > 0 || 1) {
		//printf("vmc_all.ground_att_est[PITr]=%f\n", vmc_all.ground_att_est[PITr]);
		vmc[leg_sel_trig].epos_spdd_n.x += cosd(g_att)*dx - sind(g_att)*dz;
		vmc[leg_sel_trig].epos_spdd_n.z += sind(g_att)*dx + cosd(g_att)*dz;
	}

	vmc[leg_sel_trig].epos_spdd_n.z += LIMIT(vmc_all.spd_n.z, -max_end_spd / 2, max_end_spd / 2);
#else
	//if(vmc[leg_sel_trig].param.trig_state==3)
#if 1
	if (vmc[leg_sel_trig].param.trig_state >= 3 && 1) {//check spd flag

		vmc[leg_sel_trig].epos_spdd_n.z += LIMIT(vmc_all.spd_n.z, -max_end_spd, max_end_spd);
	}
	else {

		vmc[leg_sel_trig].epos_spdd_n.z += LIMIT(vmc_all.spd_n.z, -max_end_spd, max_end_spd);
	}

	if (0 && vmc[leg_sel_trig].param.trig_state == 2 && (HU_TASK == HU_TASK2 || HU_TASK == HU_TASK4)) {
		vmc[leg_sel_trig].epos_spdd_n.x -= LIMIT(vmc_all.spd_n.x, -max_end_spd, max_end_spd);
		vmc[leg_sel_trig].epos_spdd_n.y -= LIMIT(vmc_all.spd_n.y, -max_end_spd, max_end_spd);
	}
#endif
#endif

#if 1
	Vect3 Hip_b;
	Vect3 com;
	com.x = com.y = com.z = 0;
#if 1
	Hip_b.x = com.x - vmc[leg_sel_trig].epos_b.x;
	Hip_b.y = com.y - vmc[leg_sel_trig].epos_b.y;
	Hip_b.z = com.z - vmc[leg_sel_trig].epos_b.z;
#else
	Hip_b.x = com.x - (vmc[leg_sel_trig].flag_fb*Hw / 2);
	Hip_b.y = com.y - (vmc[leg_sel_trig].flag_rl*Www / 2);
	Hip_b.z = com.z - 0;
#endif
	Vect3 w_b;
	w_b.x = my_deathzoom(vmc_all.att_rate_swing[ROLr], 0) * 0.0173 * 1 * 1;  //需要补偿地形角的角速度吗？
	//w_b.y = LIMIT(my_deathzoom(vmc_all.att_rate_trig[PITr], 5),-25,25) * 0.0173 * -1*1;
	w_b.y = (my_deathzoom(vmc_all.att_rate_swing[PITr], 0)) * 0.0173 * 1 * 1;
	//printf("%f %f\n",vmc_all.att_rate_trig[PITr],vmc_all.att_rate_trig[ROLr]);
	w_b.z = vmc_all.att_rate_swing[YAWr] * 0.0173 * 1;//加上上台阶好一些
	float w_cross_b[3][3] = { 0 };
	vect3_2_cross(w_b, w_cross_b);
	Vect3 hip_rotate_spd_b, hip_rotate_spd_n, hip_rotate_spd_g;

	matrx33_mult_vect3(w_cross_b, Hip_b, &hip_rotate_spd_b);

	converV_b_to_n(hip_rotate_spd_b.x, hip_rotate_spd_b.y, hip_rotate_spd_b.z,
		&hip_rotate_spd_n.x, &hip_rotate_spd_n.y, &hip_rotate_spd_n.z);//转换速度直接输出到IMP  控制频率不够目前？？

	converV_n_to_g(hip_rotate_spd_n, &hip_rotate_spd_g);//不加不行

	vmc[leg_sel_trig].epos_spdd_n.x += hip_rotate_spd_g.x * -1;
	vmc[leg_sel_trig].epos_spdd_n.y += hip_rotate_spd_g.y * -1;
	vmc[leg_sel_trig].epos_spdd_n.z += hip_rotate_spd_g.z * -1;
#endif
	vmc[leg_sel_trig].epos_spdd_n.x = LIMIT(vmc[leg_sel_trig].epos_spdd_n.x, -max_end_spd, max_end_spd);
	vmc[leg_sel_trig].epos_spdd_n.y = LIMIT(vmc[leg_sel_trig].epos_spdd_n.y, -max_end_spd, max_end_spd)*(vmc_all.param.leg_dof == 3);
	vmc[leg_sel_trig].epos_spdd_n.z = LIMIT(vmc[leg_sel_trig].epos_spdd_n.z, -max_end_spd, max_end_spd);


#if 0
#if F_SWING_WITH_ROLL
	vmc[leg_sel_trig].epos_spdd_n.z /= cosd(LIMIT(vmc_all.att_trig[ROLr], -ROLL_LIMIT_COM, ROLL_LIMIT_COM));
#endif
	converV_n_to_b(vmc[leg_sel_trig].epos_spdd_n.x, vmc[leg_sel_trig].epos_spdd_n.y, vmc[leg_sel_trig].epos_spdd_n.z,
		&vmc[leg_sel_trig].epos_spdd_b.x, &vmc[leg_sel_trig].epos_spdd_b.y, &vmc[leg_sel_trig].epos_spdd_b.z);//转换速度直接输出到IMP  控制频率不够目前？？
#else
	converV_n_to_b(-vmc[leg_sel_trig].epos_spdd_n.x, -vmc[leg_sel_trig].epos_spdd_n.y, -vmc[leg_sel_trig].epos_spdd_n.z,
		&vmc[leg_sel_trig].epos_spdd_b.x, &vmc[leg_sel_trig].epos_spdd_b.y, &vmc[leg_sel_trig].epos_spdd_b.z);//转换速度直接输出到IMP  控制频率不够目前？？
	//converV_n_to_b(-vmc[leg_sel_trig].param.tar_acc_n.x, -vmc[leg_sel_trig].param.tar_acc_n.y, -vmc[leg_sel_trig].param.tar_acc_n.z,
	//	&vmc[leg_sel_trig].param.tar_acc_b.x, &vmc[leg_sel_trig].param.tar_acc_b.y, &vmc[leg_sel_trig].param.tar_acc_b.z);//转换速度直接输出到IMP  控制频率不够目前？？
#endif
	//vmc[leg_sel_trig].epos_spdd_b=vmc[leg_sel_trig].epos_spdd_n;
	vmc[leg_sel_trig].tar_epos_h.x += -vmc[leg_sel_trig].epos_spdd_b.x*dt;//积分摆动轨迹  用于作为IMP的期望轨迹！！！
	vmc[leg_sel_trig].tar_epos_h.y += -vmc[leg_sel_trig].epos_spdd_b.y*dt;
	vmc[leg_sel_trig].tar_epos_h.z += -vmc[leg_sel_trig].epos_spdd_b.z*dt;
#if 1
	vmc[leg_sel_trig].tar_epos_h.x = LIMIT(vmc[leg_sel_trig].tar_epos_h.x, MIN_X, MAX_X);//位置限制幅度  输出到IMP摆动控制
	vmc[leg_sel_trig].tar_epos_h.y = LIMIT(vmc[leg_sel_trig].tar_epos_h.y, MIN_Y, MAX_Y);
	vmc[leg_sel_trig].tar_epos_h.z = LIMIT(vmc[leg_sel_trig].tar_epos_h.z, MAX_Z, MIN_Z);//位置限制幅度
#endif
	espd_to_qspd(&vmc[leg_sel_trig], vmc[leg_sel_trig].epos_spdd_b, vmc[leg_sel_trig].param.spd_dj, dt);
	//espd_to_qspd(&vmc[leg_sel_trig], vmc[leg_sel_trig].param.tar_acc_b, vmc[leg_sel_trig].param.acc_dj, dt);

#if 0//逆解直接计算角度 全局位置fail
	converV_n_to_b(vmc[leg_sel_trig].param.tar_epos_n.x, vmc[leg_sel_trig].param.tar_epos_n.y, vmc[leg_sel_trig].param.tar_epos_n.z,
		&vmc[leg_sel_trig].param.tar_epos_b.x, &vmc[leg_sel_trig].param.tar_epos_b.y, &vmc[leg_sel_trig].param.tar_epos_b.z);

	converV_b_to_leg(leg_sel_trig, vmc[leg_sel_trig].param.tar_epos_b.x, vmc[leg_sel_trig].param.tar_epos_b.y, vmc[leg_sel_trig].param.tar_epos_b.z,
		&vmc[leg_sel_trig].tar_epos_h.x, &vmc[leg_sel_trig].tar_epos_h.y, &vmc[leg_sel_trig].tar_epos_h.z);

	vmc[leg_sel_trig].tar_epos_h.x = LIMIT(vmc[leg_sel_trig].tar_epos_h.x, MIN_X, MAX_X);//位置限制幅度  输出到IMP摆动控制
	vmc[leg_sel_trig].tar_epos_h.z = LIMIT(vmc[leg_sel_trig].tar_epos_h.z, MAX_Z, MIN_Z);//位置限制幅度

	inv_end_state_new(&vmc[leg_sel_trig],//已经转换内部调用函数
		vmc[leg_sel_trig].tar_epos_h.x,
		vmc[leg_sel_trig].tar_epos_h.y,
		vmc[leg_sel_trig].tar_epos_h.z,
		&vmc[leg_sel_trig].tar_sita1,
		&vmc[leg_sel_trig].tar_sita2,
		&vmc[leg_sel_trig].tar_sita3);
#elif SWING_USE_IK//逆解直接计算角度 机体速度good
	inv_end_state_new(&vmc[leg_sel_trig],//已经转换内部调用函数
		vmc[leg_sel_trig].tar_epos_h.x,
		vmc[leg_sel_trig].tar_epos_h.y,
		vmc[leg_sel_trig].tar_epos_h.z,
		&vmc[leg_sel_trig].tar_sita1,
		&vmc[leg_sel_trig].tar_sita2,
		&vmc[leg_sel_trig].tar_sita3);
#elif !SWING_USE_SPD_MODE//雅克比直接换算角度输出goodwithnofb
	espd_to_qspd(&vmc[leg_sel_trig], vmc[leg_sel_trig].epos_spdd_b, vmc[leg_sel_trig].param.spd_dj, dt);

	vmc[leg_sel_trig].tar_sita1 += vmc[leg_sel_trig].param.spd_dj[0] * k_spd_err*dt;//�ٶ����
	vmc[leg_sel_trig].tar_sita2 += vmc[leg_sel_trig].param.spd_dj[1] * k_spd_err*dt;
	vmc[leg_sel_trig].tar_sita3 += vmc[leg_sel_trig].param.spd_dj[2] * k_spd_err*dt;
#endif
}

void reset_tar_pos(char leg_sel_trig) {//区分着地离地？<<----------------------------!!!!!!!!!!!!!!!!
	vmc[leg_sel_trig].param.tar_epos_n = vmc[leg_sel_trig].epos_n;
	vmc[leg_sel_trig].param.tar_epos_b = vmc[leg_sel_trig].epos_b;//摆动N和B系轨迹  Trot
	vmc[leg_sel_trig].param.tar_epos_h = vmc[leg_sel_trig].eposc;//H系  阻抗位置模式的角度  复位阻抗
	vmc[leg_sel_trig].param.tar_epos_h_reg = vmc[leg_sel_trig].eposc;//之前赋值反了导致着地后跳变Bug0001  unuse now
	vmc[leg_sel_trig].tar_epos_h_reg = vmc[leg_sel_trig].eposc; 
	vmc[leg_sel_trig].tar_epos_h = vmc[leg_sel_trig].eposc;//站立 末端逆解和摆动逆解 规划结果纯 需要滤波到H输出 nouse

	vmc[leg_sel_trig].param.spd_dj[0] = vmc[leg_sel_trig].param.spd_dj[1] = vmc[leg_sel_trig].param.spd_dj[2] = 0;
	vmc[leg_sel_trig].param.spd_dj[3] = vmc[leg_sel_trig].param.spd_dj[4] = 0;
	vmc[leg_sel_trig].tar_sita5 = vmc[leg_sel_trig].sita5;
}

void trig_plan_online(char leg_sel_trig, float dt) {
	cal_tar_end_pos_n_v1_sel(leg_sel_trig, 1, dt);//
	vmc[leg_sel_trig].tar_pos = vmc[leg_sel_trig].tar_epos_n;//
#if EN_PLAN_USE_JERK
	if (EN_PLAN_USE_JERK == 5)
		swing_jerk_planner_5point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
	else
		swing_jerk_planner_3point(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
#else
#if EN_PLAN_USE_MIT
	swing_Bezier_planner(&vmc[leg_sel_trig], lf_spd, td_spd, sw_z[leg_sel_trig], sw_t[leg_sel_trig] - st_t[leg_sel_trig] - lf_time - td_time);
#else
	cal_curve_from_pos_new(&vmc[leg_sel_trig], vmc[leg_sel_trig].tar_pos, sw_z[leg_sel_trig], sw_t[i] - st_t[i] - lf_time - td_time);
#endif
#endif
}
